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SUMMARY 

An optimized resolved rate control scheme for real-time control of seven-degree-of-freedom 
manipulators with spherical wrists was recently developed at the Oak Ridge National Laboratory. 
The scheme generates a least-squares solution for joint angle rates, which move the robot hand at a 
commanded velocity while (optionally) trying to configure the arm to satisfy a specified performance 
criterion of joint angles. 

The present paper discusses the use of this scheme for controlling a prototype of a seven-degree- 
of-freedom robot arm — the Laboratory Telerobotic Manipulator (LTM) — built by the Oak Ridge 
National Laboratory for NASA. Axis systems and homogeneous transformation matrices based on 
the Denavit-Hartenberg parameters are established for the LTM along with appropriate resolved 
rate control equations. When the optimization scheme cannot be used due to kinematic singularities 
that result in less than six degrees of freedom, special resolved rate equations are also presented. 

A three-dimensional graphics model of the LTM was driven by velocity commands from a six-axis 
hand controller to assess the equations developed in this paper. For the motions simulated, the robot 
hand moves as commanded and the special resolved rate equations for kinematic singularities appear 
reasonable. 

INTRODUCTION 

The Laboratory Telerobotic Manipulator (LTM) is a seven-degree-of-freedom robot arm built by 
the Oak Ridge National Laboratory for NASA. A prototype structure with two of the arms (fig. 1(a)) 
has been delivered to the Langley Research Center for evaluation in ground-based research to assess 
the role of redundant degree-of-freedom arms in space operations. Each arm has three pitch-yaw 
joints (fig. 1(b)); one at the shoulder, another at the elbow, and a third at the wrist. The seventh 
degree of freedom is provided by a wrist roll joint. The problem of interest in this paper is velocity 
control of the LTM. 

Velocity control is a popular way to control robot arms. The velocity of the robot hand is known 
(or commanded by an operator) and this velocity is resolved into joint angle rates (resolved rate 
control, ref. 1) to move the hand as commanded. Six independent joints are all that are needed 
for general movement of the hand in its workspace; but the LTM has a seventh joint, which means 
there are more choices for joint angle rates to move the hand as commanded. A reasonable solution 
from among the many choices is one for which the sum of the squares of the joint angle rates is the 
smallest — a least-squares solution. The solution may also be a trade-off between small rates and 
those rates which work toward satisfying a specified performance criterion of the joint angles. 

An optimized resolved rate control scheme for seven-degree-of-freedom manipulators with spher- 
ical wrists (called Dubey’s method in this paper (ref. 2)) is applied to the LTM. Dubey’s method 
generates a least-squares solution for the joint angle rates which move the robot hand at the com- 
manded velocity while (optionally) trying to configure the arm to satisfy a specified performance 
criterion of joint angles. In essence, Dubey’s method is a fast way to compute the least-squares so- 
lution for arms like the LTM. Real-time control was the impetus for the development of the scheme. 

A problem with using Dubey’s method is that it requires selecting a column of the Jacobian 
matrix so that the remaining six columns (or, equivalently, six joint angle rates) are independent. 
A method to determine quickly which column to select is presented in this paper and is different 
from the approach in reference 3, a recent application of the scheme to the LTM. However, such a 
selection is not always possible — in which case the scheme does not apply — and alternate control 
equations are devised. 

SYMBOLS 

A j homogeneous transformation matrix from axis system i to axis system j 

a{ Denavit-Hartenberg parameter, perpendicular distance between i and 

Zi 

Ci cos Qi 

d{ Denavit-Hartenberg parameter, distance between coordinate systems i — 1 

and i along \ 
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scalar-valued performance criterion 
gradient of H(0) 
gradient defined in equation (9) 
gradient defined in equation (10) 
identity matrix 

Jacobian matrix (6 by 7) --- -- ------ 

generalized inverse of J 

submatrices of J (see eq. (8) or eq. (D2)) 

submatrix (3 by 3) of Ji 

constant which determines rate of convergence to H(0) 
length from elbow to shoulder (constant) 
length from hand to wrist (constant) 
length from wrist to elbow (constant) 
line-of-sight distance from wrist to shoulder 

index which indicates component of <^p,arm and set to 0 and 1, 

respectively, when using Dubey’s method 

position vector from joint axis system i — 1 to joint axis system i\ expressed 
in joint axis system i — 1 

rotational part of homogeneous transformation matrix from axis system i 
to axis system j 

rotation matrix (3 by 3) representing a rotation about 5 axis by angular 
amount ip 

sin 8 i 

time, sec 

commanded translational velocity of robot hand 

thrusting, pitching, and rotating velocities of hand relative to line-of-sight 
from shoulder to wrist, used in forming equations to fully extend robot 
arm when elbow pitch angle #3 is involved 

thrusting, pitching, and rotating velocities of hand relative to line-of-sight 
from shoulder to wrist, used in forming equations to fully extend robot 
arm when elbow yaw angle #4 is involved 

axes associated with joint i + 1, is axis of rotation; also, unit vector 
along axis 

corresponding axes after rotation of axis system (X2,Y2,Z2) by angle ji 
about Y2 ; when both elbow joints are ±90°, velocity cannot be produced 
along 

corresponding axes after rotation of axis system {X^Y^.Z^) by angle 
v about Z^\ with loss of shoulder and elbow pitch, velocity cannot be 
produced along Y v 

coordinates 
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X 

a; 

Pi 
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0arm 

®opt 

0opt,arm 

0\vrist 
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<ri 

<?2 
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<t>h 

0ft, arm 

0ft, arm 
0ft, wrist 

k 

0p 

0p,arm 

0p,arm 


commanded velocity (translational and rotational) of robot hand 

Denavit-Hartenberg parameter, angle between Z{-\ and Zj, measured 
positively about positive X; 

constant joint angle offset bias 

small positive angles used to specify singularity regions 

angle associated with joint i (0j = 0° for i = 1 to 7 in fig. 2) 

Denavit-Hartenberg parameter, joint angle between X*_i and Xj, mea- 
sured positively about positive Z{-\ 

last values of 0 3 and 0 4 , respectively, before singularity region of special 
solution 1 was entered (fig. 6), initialized to initial values of 0 3 and 0 4 

vector of seven joint angle rates 

Euclidean norm of 6 

vector of four arm joint angle rates 6\, 02 , 03 ? aR d #4 
optimized vector of seven joint angle rates 
optimized vector of four arm joint angle rates 
vector of wrist joint angle rates 05 , 06, and 07 

angle between elbow- to-shoulder link and line-of-sight from shoulder to 
wrist (fig. 3) 

angle between and Z p x X 3 , used in showing velocity component that 
cannot be produced with loss of shoulder and elbow pitch (defined by 
eqs. (81) and (82)) 

angle from plane of Xo and Zq to line of sight from shoulder to wrist when 
elbow is not yawed (fig. 5(a)) 

angle from X\ axis to line of sight from shoulder to wrist when elbow is 
not pitched (fig. 5(b)) 

angle between elbow-to-shoulder link and line of sight from shoulder to 
wrist when elbow is not yawed (fig. 5(a)) 

angle between elbow-to-shoulder link and line of sight from shoulder to 
wrist when elbow is not pitched (fig. 5(b)) 

vector of joint angle rates (eq. (4)) 

homogeneous solution of equation (4) 

vector of four rates 0/Jl], 0ft [2], 0ft [3], and 0ft [4] 

subvector of 0^ a rm 

vector of three rates 0ft [5], 0ft [6], and 0ft [7] 

component i of 0 

particular solution of equation (4) 

vector of four rates 0 P [1], 0 P [2], 0 p [3], and 0p[4] 

subvector of 0 p ,arm 



&P , wrist 


vector of three rates $ p [ 5 ], <j> p [ 6], and 4 > p [ 7 ] 
u; commanded rotational velocity of robot hand 

3 -D three-dimensional 

Arm reference points: 

E elbow 

H hand 

S shoulder 

W wrist 

A dot over a quantity indicates the derivative with respect to time. A caret ( A ) over a vector 
indicates that the vector is expressed in base coordinates (Xq.Yq.Zq). The component index of a 
vector is listed as a subscript or in brackets ([ ]). If a vector has a label subscript, the component 
index is always listed in brackets. For example, the second component of the joint angle rate vector 
0 is #2 or 0 [ 2 ]. The second component of the arm joint angle rate vector 0 arm is ^arm[2]’ 

DESCRIPTION OF APPENDIXES 

To reduce material in the main body of the paper and to present other pertinent information, 
six appendixes have been included at the end of this paper. Appendix A lists the homogeneous 
transformation matrices A|_ 1? from joint axis system i to joint axis system i— 1 and presents a fast 
way to compute the composite transformation matrix from the hand axis system to the base axis 
system (hand-to-base transformation Aq). Appendix B extends the usefulness of the equations in the 
analysis by showing that operator inputs need not be restricted to the hand axis system (at wrist). 
Appendix C derives an example performance criterion which may be used to encourage movement 
away from some of the singularities of the LTM. Appendix D presents a method for computing 
quickly several submatrices of the Jacobian matrix in desired reference frames. The solution for the 
wrist joint angle rates is given in a generic form, which applies directly to four other similar equations 
in the analysis. Also, a means of dealing with the wrist singularity is presented. In appendix E an 
expression for a submatrix of the Jacobian is derived in the axis system of the second joint (shoulder 
yaw 62 ) for analyzing the singularities of the LTM. Appendix F describes a computer program used 
to implement the equations in this paper. 

ANALYSIS 

The LTM with axis systems is depicted in figure 2 , where all joint angles are defined to be zero. 
In this initial position, 6\, 9 3, and #5 produce a pitching motion of the hand; 62, #4, and 9 q produce 
a yawing motion of the hand; and Oj rolls the hand. As the LTM moves, the X2 axis is always 
aligned with the elbow- to-shoulder link; the X4 axis is always aligned with the wrist-to-elbow link; 
and the Zq axis (and Z 7 axis) is always aligned with the hand-to-wrist link. By convention, 9 j 
rotates about Z ? _p Denavit-Hartenberg parameters for the LTM are listed in table I. Homogeneous 
transformation matrices based on these parameters are given in appendix A. 

The axis system chosen for operator inputs is the robot hand axis system {X7X1 ^Zj), although 
this need not be true. (See appendix B.) A simplifying assumption is that = 0 to locate the 
origin of the hand axis system at the intersection of the rotational axes of the robot wrist. Thus, 
wrist rotation ($5, 9 §, and 9 7) does not translate the origin of the hand axis system. 

A robot needs only six independent degrees of freedom to translate and orient its hand, so there 
is a redundant degree of freedom among the seven joints of the LTM. This redundant degree of 
freedom means different configurations of the arm can be used to produce identical motions of the 
hand (which can be important, for example, when reaching around obstacles), but the redundancy 
complicates real-time control. 
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Joint angle rates of a robot arm cause the hand to move at a velocity 

x = JO 


( 1 ) 



where J is the Jacobian matrix and 9 is a vector of joint angle rates. The idea of resolved rate 
control is to specify x and calculate 9. 

Performance Criterion 

A solution of equation (1), optimized by a specified scalar- valued performance criterion H(0) is 
(ref. 4) 


0 op t = J+i + Jfc [i - J+ j] VH 

= J + [x - fcJVH] + fcVH (2) 

where 0 op t is the vector of optimized joint angle rates, J + is the generalized inverse of the Jacobian 
matrix, I is the appropriate identity matrix, and VH is the gradient of the performance criterion. 
In this paper, J+ is computed by singular value decomposition of J. The constant scalar k in 
equation (2) is a weighting factor which determines the trade-off between a least-squares solution 
for the joint angle rates (J + x), and a solution which optimizes the performance criterion H(0). As 
\k\ 0, preference is given to minimizing joint angle rates. If the goal is to maximize H(0), k should 

be positive; if the goal is to minimize H (0), k should be negative. An example performance criterion 
which may be used to avoid some of the singularities of the LTM is discussed in appendix C. 

Dubey’s Method of Computing Optimized Joint Angle Rates 

The method in reference 2 for calculating optimized joint angle rates for seven-degree-of-freedom 
robot arms (called Dubey’s method in this paper) assumes that equation (2) can be expressed in 
the form 

0opt = + *vh (3) 

<Ph * <Ph 

where <p p and 0^ are particular and homogeneous solutions, respectively, of the equation 


x — £JVH — J0 (4) 

(The reader should note the following nomenclature: Joint angle rates which are solutions of 

equation (1) are always denoted 9. The Jacobian matrix and the performance criterion are both 
functions of the joint angle vector 9, formed by integrating 0. Particular and homogeneous solutions 
of equation (4) are denoted 0 p and 0/^ respectively, and are used to calculate an optimized 9.) 
Specifically, the particular solution 0 p satisfies 


x - JfcJVH = J0 p (5) 

with one component of 0 P assigned the value 0 by choice, and the homogeneous solution satisfies 
the equation 

0 = J 4> h (6) 

with one component of 0^ assigned the value 1 by choice. The component assigned the value 0 in 
0 p corresponds to the component assigned the value 1 in 0/*. Once the particular and homogeneous 
solutions are found, the homogeneous solution is multiplied by a scalar (the dot product of the two 
solutions divided by the dot product of the homogeneous solution with itself) and is subtracted from 
the particular solution. The gradient of the performance criterion times the rate-of-convergence 
constant is added to the result to form the optimized solution (eq. (3)). Dubey’s method implicitly 
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assumes full rank for the 6 by 6 Jacobian submatrix which must be inverted in equations ( 5 ) and ( 6 ) 
to find the six unknown joint angle rates, respectively, of the particular and homogeneous solutions. 
An alternative method must be used if the Jacobian matrix has rank five or less (i.e., if the robot 
loses the ability to move along or rotate about some direction in Cartesian space). The advantage 
of Dubey’s method is that the optimized solution can be found without formally computing the 
generalized inverse. For a more detailed discussion of Dubey’s method, see references 2 and 5 . The 
remainder of this analysis presents an efficient method for using Dubey’s method to control the LTM 
and suggests some alternative solutions when the method cannot be used. However, these alternative 
solutions are by no means the only possible approaches for coping with singular configurations of 
the arm in which Dubey’s method does not apply. 

Particularized Algorithm for Dubey’s Method 

The vector x in equation ( 4 ) is partitioned as 


x = 



( 7 ) 


where V and w are commanded translational and rotational velocity vectors, respectively, of the 
hand axis system. Since the hand axis system is assumed to be located at the wrist in this paper, 
the wrist produces only rotational motions of the hand axis system, and equation ( 4 ) can be written 
as 


V 


Ji ! o 
J2 j J3 


VH arm 

^Hwrist 


Jl 

0 

f 0arm 1 

— 

— 

| ) 

_J 2 

I 

1 J 3J 

V 0wrist J 


(8) 


where 


VH arm 


dK/ddx >1 

du/de 2 I 
dn/ee 3 [ 
du/de 4 ) 


f dE/de b } 
vH wrist = { aafdOe \ 
[ dn/dd 7 J 


0 arm 



( 9 ) 


( 10 ) 


( 11 ) 


0wrist — 



( 12 ) 


With respect to translational motion of the hand (the V-part of eq. ( 8 )), any redundancy must 
reside in the first four joints, since the three wrist joints cannot translate the hand (at wrist). 

Dubey’s method relies on being able to solve for six independent joint angle rate components, 
each of which is either a component of </> arm or 0 wr i s t- If J3 is not singular, the solution for 0 wr ist 
provides three of these components. The other three independent components must come from the 
four components of 0 arm , provided Ji has full rank. 

Therefore, if J3 is not singular and if Ji has full rank, one of the components of <jf> a rm is chosen 
to be 0 and 1 , respectively, in forming the particular and homogeneous solutions of equation ( 8 ). 
The details of calculating these particular and homogeneous solutions follow. 
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Computation of the Particular Solution 

Equation (8) allows formation of the particular solution from the translational and rotational 
velocity equations: 


V — fcJlVHarm — Jl0p,arm 


a; - J 2 


0p ,arm + fcVHarm “ — J 3 <t> p , wrist 


(13) 

(14) 


Equation (13) is solved for (f> PATm with one component assigned the value 0 by choice. Once <fi p ^ r m 
is calculated, equation (14) is solved for <f> PtVfT i s t* 

For discussion purposes, set the mth component of <f> p ,arm 1° 0 m equation (13) and delete the 
mth column of Jp Let the remaining submatrix of Ji be denoted by Ji and let the vector of the 

remaining three components of 0p,arm be denoted by <f> p ,a.im • Assuming that J 1 is invertible, the 
reduced equation yields 

0p,arm = [V — fcJiVH arrn ] (1^) 

By properly associating indices, one can assemble 0p,arm from the components of 0p,arm with the 
missing component given by 0 p , a rm[ra] = 0. 

Once 0p,arm is known, equation (14) can be solved by 


^p, wrist “ ^3 


\U) 


"" J 2 ^0p, arm d - &VH arm ^ ^J3^^wristj 


(16) 


Equation (16) formally represents the solution of equation (14), but simple expressions are derived 
in appendix D to expedite the calculation of <t> PiWT \ s t and deal with the singularity of J3. Finally, 
the complete particular solution is 



(17) 


Computation of the Homogeneous Solution 

Equation (8) allows formation of the homogeneous solution from the two equations: 

0 = Ji</>/i ? arm (^®) 

“J2 4>h ,arm J3 <t>h , wrist (19) 

Equation (18) can be expressed in the form: 

j f Ji[l,m] ] 

0 = Jl^/^arm "h \ Jl [2, mj > 0/i,arin[ m ] (20) 

( Ji[3,m] J 

where removing the mth column of Ji leaves the submatrix J, and removing the mth component 
from (j>h,a.Tm leaves <t>h, arm- 

Recall that the value 1 is assigned to the mth component of the homogeneous solution in Dubey’s 
method. Consequently, with the assignment 0/ J) arm[ m ] = 1, the solution of equation (20) is 


tyh, arm 




( 21 ) 
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By properly associating indices, one can assemble <j>h,arm from the components of 4>h,a.vm t with the 
missing component given by 4>h, axm[ m ] = 1 - Note that Jj -1 appears in both equations (15) and (21). 
Once ' s known, the solution of equation (19) is 


$h, wrist ^3 ^20ft,armj 


( 22 ) 


Equation (22) formally represents the solution of equation (19), but simple expressions are derived 
in appendix D to expedite calculation of <t>h, wrist an d to deal with the singularity of J3. Finally, the 
complete homogeneous solution is 



(23) 


The solutions of equations (13), (14), (18), and (19) together with equations (17), (23), and 
(3) thus reduce the problem of computing an optimized solution to equation (1) to the problem of 
inverting two 3 by 3 matrices — Ji in equations (15) and (21) and J3 in equations (16) and (22). 
However, it is not always possible to find an invertible Ji, nor is it always possible to invert J3. The 
method outlined in this section for generating an optimized solution to equation (1) is used only if 
an invertible Ji exists and J3 is also invertible. Cases in which Dubey’s method cannot be used are 
discussed later. 


Determining an Invertible Ji 


The matrix Ji for the LTM expressed in the ( A' 2 , V 2 , Z 2 ) axis system is 


Jl 


-CiS 3 C 2 lwE -SaIwE -C4S3IWE 

C2IES + (C4C3C2 _ S/[S2)Iwe 0 CaC^Iwe 

-C^S^Iwe -IeS - C 4C3IWE 0 


-S 4 C 31 WE 

-S 4 S 31 WE 

-c^lwE 


(24) 


as derived in appendix E. The lengths of the elbow- to-shoulder link (Ies) and the wrist-to-elbow 
link (Iwe) are shown in table I. Table II lists the determinants and associated singularities for each 
of the four possible 3 by 3 submatrices (Ji) that can be formed by deleting a column from Jj. Any 
submatrix J 4 can always be inverted as long as its determinant is not zero (which occurs at its 
singularities). Although shown in table II, the singularity conditions |#3| = 180° and |0 4 | = 180° are 
not considered physically realizable and are ignored. In this paper, mutually exclusive partitions of 
the motion of 9 4 are used to decide which column of Ji to delete. These partitions are (1) |0 4 | ^ 0° 
and |0 4 | / 90°; (2) |0 4 | = 0°; and (3) |0 4 | = 90°. 

The equalities and inequalities in the following discussion are treated in a strict sense. But, on 
a computer, these equalities and inequalities must be defined in terms of regions. Appropriate sizes 
for the regions are discussed in appendix F. 

Partition (1): |0 4 | 7^ 0° and |0 4 | ^ 90° 

Notice in table II that after column 1 of Ji is deleted the remaining J 4 can always be inverted 
as long as |0 4 | is not 0° or 90°. Therefore, if |0 4 | is neither 0° nor 90°, the index m used in forming 
the particular and homogeneous solutions is chosen as m = 1 (i.e., 0p, arm [ 1] = 0 and ^armfl] = 1)- 

Partition (2): |0 4 | = 0° 

If |0 4 | = 0°, then neither the elimination of column 1 nor column 3 from J 4 will result in an 
invertible J 4 . However, it is possible to eliminate column 2 to produce an invertible J 4 if |02| / 90° 
and |03| # 0°. Therefore, when |0 4 | = 0°, if |^2 1 i 1 90° an d l^3l 7^ 0°, the index m used in forming 
the particular and homogeneous solutions is chosen as m = 2 (i.e., </> p , arm[2] = 0 and ^ iarm [2] = 1). 
If |02| = 90° or 1 03 1 = 0° then it is not possible to form an invertible J 4 from J 4 . 
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Partition (3): \6^\ = 90° 

If |&i| = 90°, then neither the elimination of column 1, column 2, nor column 4 from Ji will 
result in an invertible Jj. However, it is possible to eliminate column 3 to produce an invertible Jj 
if |0 3 J ± 90° and -IesC 2 ^ Iwe(C a C z C 2 - S 4 S 2 ). 

Since |^4 j = 90°, -IesC 2 # ^we{^aCzC 2 - S$S 2 ) simplifies to IesC 2 ^ sign^)/^^ where the 
sign function is defined as: 

sign(x)={+; £;$} 05) 

where x is a general argument. Geometrically, the equality condition IesC 2 = sign(S 4 )lwES 2 
holds when the arm lies in the plane of Zq and X\ with the wrist located on Zq, as shown in 
figure 3. As discussed in a later section, it is more convenient to describe the inequality condition 
IeS^2 # sign(S , 4)/i4E'.5'2 in terms of the shoulder joint angle 6 2 . Toward this end, the angle between 
the elbow-to-shoulder link and the line-of-sight distance from the shoulder to the wrist (Iws) when 
1^4 1 = 90° is called n and is defined to be positive when 6± — 90° (as in fig. 3). Mathematically, 
IesQ / sign(54)/H®S , 2 if and only if \0 2 + (i\ / 90°: 


sin /r = 


cos fJL = 


sign(S 4 )%E 

\Ae S + l WE 

l ES 


(Note that |/x| is constant, and in this paper, |^| = arctan(20/23) = 41.01°.) Then, 

cos(#2 + /r) = C2 cos fi — S 2 sin /i 

_ IesQ 2 - sign(5 , 4 )/ WE 5 , 2 


(26) 

(27) 


Therefore, the condition IesC 2 / sign (.94)/^- 52 is equivalent to the condition cos (#2 + fi) / 0 or 


(28) 


1^2 + lA # 90° (29) 

When |^4 1 = 90°, if 1 6^3 [ / 90° and \6 2 + fi\ ^ 90°, the index to used in forming the particular 
and homogeneous solutions is chosen as to = 3 (i.e., (f> P ^ rm [3] = 0 and 0/, jarm [3] = 1). If |0 3 | = 90° 
or \0 2 + /x| = 90°, then it is not possible to form an invertible Ji from J 1 . 

Figure 4 is a flowchart which uses the logic just described to determine which column to eliminate 
from Ji to form an invertible Jj (or which component to set to 0 and 1 when forming the particular 
and homogeneous solutions, respectively). Equality conditions are shown in the figure for simplicity, 
but singularity regions are defined when using the logic in a computer program (appendix F). The 
flowchart also indicates when it is not possible to form an invertible Ji and identifies the associated 
special solution to use for the arm joint angle rates (each special solution is discussed in a later 
section). 


Determining the Invertibility of J3 

The Jacobian submatrix J3 expressed in the hand axis system is derived in appendix D as 


J 3 = 


-c 7 c 6 

S 7 C 6 

-S e 


S 7 

C 7 

0 


0 

0 

1 


(30) 
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The determinant of J3 is easily 
long as 


calculated from equation (30) as 
N # 90° 


-cos 06- Hence, J3 is invertible as 

( 31 ) 


Optimizing 0 a rm When J 3 Is Not Invertible 

Even when J 3 is singular, it may still be possible to find six independent joint angle rates from 
among the components of 0 arm and 0 wrist , so that Dubey’s method could be applied. But this 
solution would involve inverting a 6 by 6 submatrix of J (after it was determined). An alternate 
solution optimizing only 0 a rm when J 1 has full rank and J 3 is singular is used in this paper. 

A region about the singularity of J3 is defined as 


||(9 6 | - 90°| < «5 6 


(32) 


where 6q is a small specified positive angle (the minimum value for <5 6 is given in appendix F). When 
an invertible Ji exists, but is inside this singularity region, only the arm joint angle rates are 
optimized: 



e = { 


• { ftp ,arm • <t>h ,arm \ 

<Pp, arm \ I 


4>h 


,arrn__ ,arrp_ 
®wrist 


+ fcVH a 


(33) 


where and 0^ ann are assembled from equations (15) and (21), along with <Ap,arm M = 0 and 

&h arm[ m j = 1- Using the method outlined in appendix D, 0 wris t in equation (33) is solved from 
equation (D2) with the substitution 0 a rm = ®opt,arm : 


UJ — J2^opt,arm — J3^wrist 


(34) 


Computing Joint Angle Rates When Dubey’s Method Does Not Apply 

In this paper, Dubey’s method does not apply when an invertible Ji does not exist. Instead, 
special solutions for the arm and wrist joint angle rates are formed from the two equations (see 
eq. (D2)): 

V = J l^iurm ( 35 > 

W — J2^arm = J3^wrist (36) 

The wrist joint angle rates are calculated by the method outlined in appendix D (regardless of 
whether J3 is invertible or not). Special solutions for 9 linn are discussed in the following section. 

Special Solutions for 0 arm When an Invertible Ji Does Not Exist 

There are four sets of configurations of the arm for which Dubey’s method does not apply because 
an invertible Ji does not exist (see fig. 4): 

1 . |f?4| = 0° and |0 3 | = 0° 

2. \e 4 \ = 0° and \6 2 \ = 90° 

3. |04 1 = 90° and |0 3 | = 90° 

4. (04 1 = 90° and |f?2 + hi = 90° 
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In these configurations, the arm cannot physically produce a component of the commanded 
translational velocity vector V, as shown later. 

Special solutions for 0 arm are used in singularity regions defined around these four conditions. 
The singularity regions are 


|04| < 84 and |03| < 63 

(37) 

|04 1 < 84 and ||^2 1 _ 90°| < ^2 

(38) 

||0 4 | — 90°| < 84 and ||03| - 90°| < 63 

(39) 

||0 4 | - 90° | < 84 and ||0 2 + n\ - 90°| < 8 2 

(40) 


where 82, 83, and 84 are small specified positive angles, and n is defined by equations (26) and (27). 
Minimum values for 62, 63, and <$4 are given in appendix F. 

For discussion purposes in the sections that follow, joints 2 and 4 are described as the shoulder 
and elbow yaw joints, respectively, and joints 1 and 3 are described as the shoulder and elbow pitch 
joints, respectively. (See fig. 2.) Motion produced by 0 2 or 0 4 is referred to as yaw motion of the 
arm, and motion produced by 0j or $3 is referred to as pitch motion of the arm. 

The four special solutions developed in the following sections are by no means the only ones 
possible nor may they be the best. However, in the tests performed with a 3-D graphics model of 
the LTM and a six-axis joystick controller, they do seem to produce reasonable responses of the arm. 
Further experimentation is needed to better evaluate the usefulness of the special solutions and to 
determine any modifications or refinements which would be beneficial. 


Special Solution 1: Full Extension via Elbow Pitch and Elbow Yaw 

Special solution 1 applies when the robot arm is at or near full extension, when 0 3 = 0° and 
0 4 = 0° (for example, as shown in fig. 2). When the arm is fully extended, joints 2 and 4 can only 
translate the robot hand in either the same or exactly opposite directions; this is also true for joints 1 
and 3. Mathematically, substituting equation (24), with 63 - O4 = 0°, into equation (35) yields 


(V X2 ) 

0 

0 

0 

0 

<v Y2 } = 

C 2 {Ies + 1 we) 

0 

l WE 

0 

\v Z 2 ) 

0 

-( l ES + l WE ) 

0 

-lWE. 




(41) 


where the components of V are subscripted to denote the (X 2 ,Y 2 ,Z 2 ) axis system. The row of zeros 
in equation (41) means that it is not possible to produce V X2 - Therefore, there are not enough 
independent arm joints to apply Dubey’s method, and special solution 1 is used. 


Overview of Proposed Solution 

A scheme to control a six-degree-of-freedom robot arm with a single-jointed elbow in the vicinity 
of full extension was devised in reference 6. This scheme is extended here to encompass the more 
complex situation of a double-jointed elbow. Special solution 1 is activated and deactivated as the 
arm moves in and out of the singularity region defined by equation (37). The pitching joint rates 0i 
and 63 are used as a pair to extend (thrust) the robot arm as far as possible, so that the hand (at 
wrist) travels along the line of sight from the shoulder to the wrist. The yawing joint rates 0 2 and 
0 4 have the same function. When the arm is fully extended, thrust motion of the hand stops. The 
extended arm can be maneuvered like a turret at the shoulder, with 0i doing the pitching and 0 2 
doing the rotating (yawing). For now, only the pair of yawing joints are used to retract the arm in 
the singularity region. When the arm is commanded to retract, the elbow bend (in yaw) is opposite 
to that on entering the singularity region. 
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Solution for Pitching Joints 

Figure 5(a) shows the geometry that is used in deriving equations for 63 (elbow pitch rate) and 
6\ (shoulder pitch rate). Notice that the elbow is not yawed (64 =0°). Equations based on this 
geometry are still used even for a slightly yawed elbow. 

Hand velocity components V T ^ 9 V> 3 , and V m . The translational velocity components V T $ 
(thrust), V/53 (pitch), and V# 3 (rotate or yaw) of the hand relative to the line of sight from the 
shoulder to the wrist (the dashed line from S to W in fig, 5(a)) are calculated by rotating the 
commanded velocity vector expressed in the {X^y^Zf) axis system by the angle cr\ about the Z 2 
axis: 

f V T3 ) [ Vx2 } 

{ V P3 / = Rot (Z 2j (Ti) < Vy 2 > 

[ v m ) { VZ2 J 


COS (J 1 

sin a j 

o' 

(V X2 ) 


— sin <t 1 

COS (T\ 

0 

\ V Y2 1 

(42) 

0 

0 

1 

{Vz2 J 



The pitching joint angle rates 9 3 and 9\ coordinate to produce a velocity along the dashed line that 
is proportional to and 9\ is used to produce Vp 3 . (Rotating (yawing) motions are produced by 
9 2 and are discussed later.) 

Expressions for cosai and sin<ri. From figure 5(a), 


cos<t 1 = /£S + /weC ' 3 

l WS 

(43) 

lwE$ 3 
sm (Ji = — 

hvs 

(44) 


where (by the law of cosines) 


l WS = \Jl\s + l WE + 2l ES l WE C 3 


(45) 


The length ly/s (wrist to shoulder) varies with #3. Physically, the origin of the wrist axis system 
cannot coincide with the origin of the shoulder axis system, so l\ys i 1 0- The link lengths Igg (elbow 
to shoulder) and l\\/£ (wrist to elbow) are constants listed in table I. 

Equation for 9 3 in singularity region . The contribution of the elbow pitch rate 9 3 in extending 
the robot arm in response to the commanded thrust velocity V73 is computed as 


h = 


-K$ V T3 sign (#3) 
0 


(V T3 > 0, 9tfl > 0) 1 
(Otherwise) J 


(46) 


where 0 3 is the value of 9 3 just before the singularity region was entered, and K3 is a specified 
positive constant, which is assumed to be unity in this paper. Equation (46) extends the arm until 
0 3 changes sign (i.e., 9 3 passes through 0° but is still approximately 0°) and then freezes 0 3 . 

Equation for b\ . The shoulder pitch angle 9\ is used to keep the hand from moving off the 
line of sight from the shoulder to the wrist by nulling the angular rate b\ due to ^3. Differentiate 
equations (44) and (45) with respect to time and form the two expressions: 


£ _ l WE f hvsQs^s - lws s - 3 

1 cos <7! ^ l^ s 


( 47 ) 
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l ws - - 


lES l WE S 303 

hvs 


With equations (43) and (48), equation (47) is written as 


o l = 


l WE 0 3 


l ES + l WE C Z 


C 3 + 


l ES l WE S 3 \ 

l ws ) 


(48) 


(49) 


Equation for p \ . The angular rate p\ pitches the robot arm in response to an operator’s command 
and is used in computing 8\. The hand at the wrist has a moment arm l\y$ relative to the shoulder, 
and the linear pitching rate Vp% (commanded) is the product of this moment arm and the angular 
pitch rate p\. Thus, 



(50) 


where l\ys is defined by equation (45). 


Equation for 8\ in singularity region . The shoulder pitch rate 8\ has two functions in the 
singularity region: (1) keep the wrist on the dashed line in figure 5(a) as 8 3 extends the wrist and 
(2) allow the operator to pitch the extended arm. 

Components of 9\ expressed in the (X2X21Z2) axis system are (see appendix A for transformation 
matrices) 


R^R 


0 

1 



(51) 


These components are shown in figure 5(a). The component — S^l produces an unwanted yaw 
velocity of the wrist, but this yaw velocity is later nulled by subtracting it from the commanded yaw 
velocity. The component Cf8\ is used to pitch the line of sight with rate p\ and to null the rate &\: 


C^l = Pl - 


(52) 


or 

9. = ^ (53) 

C2 

where and p\ are given by equations (49) and (50), respectively. Since equation (53) is singular 
when |^2 1 — 90° , it is used only when 82 is outside a region defined around |#2| = 90°: 

r 0 IN - 90°| < a 2 )1 

1 1 ^ 62 (Otherwise) J 

where A2 is a positive angle. When 60° < |02| < 120°, half or less of 8\ is in the direction of p\ — &\ 
(eq. (52)). This was the criterion used to choose A2 in equation (54). Thus, A2 = 30°. 

Subtracting Unwanted Yaw Component Due to 9\ 

From figure 5(a), the moment arm for the component — is SVvn; and th e unwanted yaw 
velocity due to 8\ is therefore — This yaw component (which is parallel to the Z2 axis) 
is subtracted from the commanded velocity V^ 2 : 

VZ2 = V Z2 - {—S2S$Iwe0i) (55) 

The adjusted commanded velocity component Vz2 is used instead of Vz2 when solutions for the 
yawing joints, 82 and #4, are computed. 


13 



Solutions for Yawing Joints 

Figure 5(b) shows the geometry that is used in forming equations for 0 4 (elbow yaw rate) and 
0 2 (shoulder yaw rate). Notice that the elbow is not pitched (03 = 0°). Equations based on this 
geometry are still used even for a slightly pitched elbow. 

Hand velocity components V TA , V P4 , and V RA . The translational velocity components V Ti , 
Vp 4 , and V RA relative to the line of sight to the wrist (the dashed line in fig. 5(b)) are calculated by 
rotating the adjusted commanded velocity vector by the angle 02 about the Y 2 axis: 


V T 4 
V P 4 
VR4 


= Rot(y 2 ,<7 2 ) | 

f V X2 1 

vy 2 
l^Z2 J 


COS <72 

0 

—sin <72 

— 

0 

1 

0 


sin <72 

0 

COS <72 



(56) 


The rotating (yawing) joint angle rates 0 4 and 0 2 coordinate to produce a velocity of the wrist along 
the dashed line in figure 5(b) that is proportional to V TA . The shoulder rotate (yaw) rate 0 2 is used 
to produce V RA . (Recall that 0 4 is used to produce a commanded pitch velocity.) 

Expressions for cos <72 and sin <r 2 . From figure 5(b), 


where (by the law of cosines) 


IeS + l * WE C 4 

COS (72 = 

l WS 

(57) 

IWES 4 
sm<72 — J 

l WS 

(58) 

= \f l ES + l WE + 2 l ES l WE C 4 

(59) 


The length Iws varies with 0 4 . Physically, the origin of the wrist axis system cannot coincide with 
the origin of the shoulder axis system; therefore, Iws f 0. 

Equation for 0 4 in singularity region. The equation for elbow yaw rate 0 4 to extend and retract 
the arm in proportion to commanded thrust velocity V PA is 


04 


0 

-K 4 V ta sign(0 4 ) 


(V TA > 0, 0 4 0| < 0) 1 
(Otherwise) 


) 


(60) 


where 6\ is the value of 0 4 just before the singularity region was entered, and K 4 is a specified 
positive constant, which is assumed unity in this paper. Equation (60) extends the arm until the 
sign of 0 4 changes, which means that 0 4 passes through 0° but is still approximately 0°. The arm 
holds this extension (but is still free to move in pitch or yaw) until a negative V T4 is commanded 
to retract the arm, whereupon the arm retracts by yawing the elbow in a direction opposite to that 
with whicli it entered the singularity region. If an elbow bend does not suit the operator, he simply 
straightens and retracts the arm again. 

Equation for <r 2 . The shoulder yaw angle 0 2 is used to keep the hand from moving off the 
line of sight from the shoulder to the wrist by nulling the angular rate <r 2 due to 0 4 . Differentiate 
equations (58) and (59) with respect to time and form the two expressions: 




l WE f %'S 04 - hvs s 4 

cos 02 \ l\- s 


(61) 
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( 62 ) 


(63) 


Equation for p2 • The angular rate P2 yaws the robot arm in response to an operator’s command 
and is used in computing 62- The wrist has a moment arm l\y§ relative to the shoulder, and the 
linear pitching rate Vp4 (commanded) is the product of this moment arm and the angular pitch rate 
P 2 ■ Thus, 



VR4 

l ws 


(64) 


where l\yg is defined in equation (59). Note that the minus sign is needed in equation (64) due to 
the assigned direction for positive p2 (fig. 5(b)). 


Equation for 82 in singularity region. The shoulder yaw rate 62 has two functions in the 
singularity region: (1) keep the wrist on the dashed line in figure 5(b) as 84 extends the hand (null 
< 72 ) and (2) allow the operator to yaw the arm according to /V> . Consequently, 


82 = Pi ~ <72 (65) 

where <72 and p2 are given by equations (63) and (64), respectively. 

Flowchart for Special Solution 1 

The joints 62 and 84 play exactly the same role as the shoulder and elbow joints, respectively, 
in reference 6. The elbow yaw rate 84 is calculated by equation (60). When positive thrust is 
commanded, 84 extends the arm until 84 changes sign (crosses 0°), and then 84 is frozen (i.e., 
84 — 0). When a negative thrust is commanded, 84 retracts the arm. The shoulder yaw rate 82 
is calculated by equation (65). The elbow pitch rate 83 is calculated by equation (46) to extend 
the robot arm when positive thrust is commanded. The shoulder pitch rate 8\ is calculated by 
equation (54). Figure 6 is a flowchart describing special solution 1. 


Special Solution 2: Full Extension via Elbow Yaw, at ±90° Shoulder Yaw 

Special solution 2 applies near the singular configuration \&2\ = 90° and 84 = 0° (for example, 
as shown in fig. 7). In this configuration, the hand velocities produced by 8 \, 82, and 84 are all 
collinear. Mathematically, substitute equation (24), with |<? 2 | = 90° and 84 — 0°, into equation (35) 
and transform the result into coordinates to get 

f ^X3 ) \ -S^WE 0 1 . 

\ V Y3 ? = R-3 0 0 CzlwE 0 0 arm 

l V Z3 ) L -si S n ( 5 '2)5 , 3/HE -(tES + C^WE) 0 ~^WE _ 


0 0 0 0 ' 

sign(S2) S^Iwe Ies + CzIwe 0 Iwe 

0 0 Iwe 0 



( 66 ) 


where R| is the transpose of R:] (see appendix A). The row of zeros in the matrix in equation (66) 
means that it is not possible to produce Vxs- Therefore, there are not enough independent arm 
joints to apply Dubey’s method, and special solution 2 is used. 


15 


Overview of Proposed Solution 

When the arm enters the singularity region defined by equation (38), the commanded velocity 
is ignored- The shoulder yaw rate 0 2 is used to produce the commanded velocity V Y3 , and the 
elbow pitch rate 0 3 is used to produce the commanded velocity V Z z- The joint angle rates Q\ and 
04 are set to zero. 

Computation of the Arm Joint Angle Rates 


Figure 7 shows the geometry that is involved in forming special solution 2. Equations are based 
on this geometry even when \0 2 \ and 0 4 are slightly away from 90° and 0°, respectively. For this 

solution, 

0i = 0 4 = 0 




Therefore, from equation (66), 


h 


Yy 3 

IeS + C3 IwE 


( 68 ) 


and 



(69) 


Transition Back to Optimized Solution 

The operator can move the arm out of the singularity region in a controlled manner by 
commanding Vy-3 (i.e., \9 2 \ moves away from 90°). Once outside the singularity region, both specia 
solution 2 and the optimized solution for the entire commanded velocity are calculated. If the sign 
of the 0 2 from the optimized solution matches the sign of 0 2 from special solution 2, control switches 
back to the optimized method and the special solution computation is stopped. If these signs do not 
match, control with special solution 2 continues. This transition back to the optimization method 
is used to prevent possible oscillations in the motion of the arm. 


Special Solution 3: Both Elbow Joints at ±90° 


Special solution 3 applies near the singular configuration |03| = 90° and |0 4 | = 90° (for example, as 
shown in fig. 8). In this configuration, 03 cannot translate the hand, and the hand velocities produced 
by 0i and 0 4 are collinear. Mathematically, substituting equation (24), with |0 3 | = |0 4 | = 90°, into 
equation (35) yields 


f V X2 ) 

S V Y2 ? = 

0 

-sign (Si)Iwe 

0 

0 


C 2 Ies ~ sign (Si)S 2 lwE 

0 

0 

-sign(S3S 4 )ZwE 

0 arm 

(V Z 2 ) 

0 

-Ies 

0 

0 



(70) 


The angle p (shown in fig. 8) was defined in equations (26) and (27), which are written here as 


where 


sign(S 4 )/yn; 
sin fi = . 

l WS 

(71) 

Ies 

COS /i = - — 

l\vs 

(72) 

l\vs = \J l ES + l WE 

(73) 
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Rotating the commanded velocity in equation (70) by the angle /x about the I 2 axis reveals that 



(V X2 ) 

Rot(r 2 ^)< Vy2 > 

l V Z2 ) 


cos /x 0 —sin /x 
0 1 0 

[sin^x 0 cos/x J 


0 


CVes _ sign(^4 )S 2 l\VE 
0 


-sign(5 4 )/H® 0 

0 0 

-Ies 0 


0 

-sign(S 3 S 4 )Jw£ 

0 


0 0 0 0 
CVes - sign(54)5 , 2/vtE 0 0 -sign^S^/^® 

0 ~l\VS 0 0 


0a 


(74) 


The row of zeros in the matrix in equation (74) means that it is not possible to produce Vy /( . 
Therefore, there are not enough independent arm joints to apply Dubey’s method and special 
solution 3 is used. 

Overview of Proposed Solution 

When the arm enters the singularity region defined by equation (39), the commanded velocity 
Vx^ is ignored. The shoulder yaw rate 62 is used to produce the commanded velocity Vz fL : and the 
elbow yaw rate 64 is used to produce the commanded velocity Vy fl - The pitch rates 6\ q.nd 0 3 are 
set to zero. 

Computation of the Arm Joint Angle Rates 

Figure 8 shows the geometry that is involved in forming special solution 3. Equations are based 
on this geometry even when |(? 3 | and |0 4 | are slightly away from 90°. For this solution, 


Therefore, from equation (74), 


and 


0 

(1 

CO 

II 

•sE* 

(75) 

02 - ‘ws 

(76) 

Vyv 

-sign(5 3 S , 4 )/ WE 

(77) 


Transition Back to Optimized Solution 

The operator can move the arm out of the singularity region in a controlled manner by 
commanding (i.e., |0 4 | moves away from 90°). Once outside the singularity region, both special 
solution 3 and the optimized solution for the entire commanded velocity are calculated. If the sign 
of 64 from the optimized solution matches the sign of 64 from special solution 3, control switches 
back to the optimized method and computation of the special solution stops. If these signs do not 
match, control using special solution 3 continues. This transition back to the optimization method 
is used to prevent possible oscillations in the motion of the arm. 

Special Solution 4: Loss of Shoulder and Elbow Pitch 

Special solution 4 applies near the singular configuration |0 4 | = 90° and |#2 + p\ — 90° (for 
example, as shown in fig. 9). In this configuration, neither 64 nor 0 3 can translate the hand (at 
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the wrist). Mathematically, substituting equation (24), with |0 4 | = 90° and |#2 + H = 90° ( or 
equivalently, C 2 IES = sign^^S^Wc) > nto equation (35) yields 


(V X2 ) 

'0 

-sign {Si)l\vE 

0 

-sign(5 4 )C3/wE 



\ V Y2 > - 

0 

0 

0 

-sign (5 4 )5 3 /h® 

^arm 

(78) 

{ V Z2 ) 

.0 

-lES 

0 

0 




The velocity component which cannot be produced by the arm in this configuration is determined 
as follows. Multiply equation (78) by the matrix Rot^./^) defined in equation (74) and substitute 
equations (71), (72), and (73) into the result to get 


(v x A 

'0 

0 

0 

— sign(5 4 )C 3 /n,E cos /i ' 


\vyA = 

0 

0 

0 

-sign(5 4 )53li^: 

$arm 

l Vz#* J 

_0 

— sign(S' 4 )/ 44 j:sin/r — cos /r 

0 

-sign(5 4 )C 3 / WE sin/r _ 




0 0 
0 0 
~hvs 0 


-sign (S^CtfwE cos M 
-sign(5 4 )5 , 3^ 


0* 


-sign(5 4 )C , 3 / H ®sin/i 


(79) 


The second column of the resulting matrix in equation (79) indicates that 82 can produce a velocity 
in the Z ^ direction only. Transforming equation (78) into the (X3,l3,Z3) coordinate system yields 


f KY3 1 


'0 

-sign (S^Iwe 

0 

-sig^^)^/^ 


{ V Y Z 

• = r 3 

0 

0 

0 

-signiS^SzlwE 

$arm 

yv Zi ) 


0 

-Ies 

0 

0 




0 


-sign(5 4 )C3/tt® 0 

lES 0 

-sign (S 4 )5 3 / m ® 0 


-sign(5' 4 )/w® 


0 

0 


(80) 


The fourth column of the resulting matrix in equation (80) indicates that 64 can produce a velocity 
in the X3 direction only. 

Since #2 can produce a velocity in the direction only and O 4 can produce a velocity in the 
direction only, the direction of the velocity which cannot be produced in the singularity [^4 1 = 90° 
and |#2 + ^1 = 90° ls the direction perpendicular to and X3 (i.e M the direction of the cross product 
of Z^ and X3). The angle between X ^ and Z ^ x X% is called v and is defined as 


cos v — 


sin v = 


-5 3 


y/S$ + Cl 


COS 2 (i 


C3 COS \X 




COS 2 /I 


(81) 

(82) 


(The expressions for sin v and cos v are derived by expressing the cross product of Z )L and X3 in 
coordinates, using the dot product of X ^ and Z ^ x X 3 to define cost/, and using the cross 
product of and xX 3 to define sin v.) Transforming equation (79) into (X l/ ,Y u ,Z l/ ) coordinates 
then yields 
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Vxn 

Rot(Z M , v) ^ V Yfi 

v Zfi 


cos v 
—sin v 
0 


sin v 0 
cos v 0 
0 1 


- 

"0 

0 

0 

-sign(5 4 )C3in® cos /x ' 


0 

0 

0 

-sign(S 4 )S3/tti; 

- 

_0 

-lws 

0 

-sig niS^CtfwE sin/x 


0 0 0 -sign(S 4 )/q£(C3COS|icosi/ + 5 3 sini/) 

0 0 0 sign(S4)/m;(C3 cos/xsini/ — S3 cos i/) 

0 -l ws 0 -sign(5 4 )C 3 Z WE sin/i 

Substituting equations (81) and (82) into equation (83) yields 


0a 


( 83 ) 


^ l = 

-0 

0 

0 

0 

0 

0 

0 

sign(S 4 )/wE \Jsl + Cl cos 2 n 

l V Zv ) 

1 

1 

0 
1 

0 

-sign(5 4 )C'3/HE sin fi 


(84) 


The row of zeros in the matrix in equation (84) means that it is not possible to produce the 
commanded velocity Vxv • Therefore, there are not enough independent arm joints to apply Dubey’s 
method and special solution 4 is used. 

Overview of Proposed Solution 

When the arm enters the singularity region defined by equation (40) the commanded velocity 
Vxv is ignored. The elbow yaw rate 84 is used to produce Vy v , and the shoulder yaw rate 82 is used 
to produce V% v . The pitch joint angle rates 8\ and #3 are set equal to zero. 

Computation of the Arm Joint Angle Rates 

Figure 9 shows the geometry that is involved in forming special solution 4. Equations are based 
on this geometry even when \&2 + p\ and \9^\ are slightly away from 90°. For this solution, 


81 = 0 3 = 0 


( 85 ) 


Therefore, from equation (84), 


^ = VYu 

sign (S 4 )i m fs[+cl COS 2 pi 

and 

k V Zy + sign(S4)Cyw£04 sin// 
h= 


( 86 ) 


(87) 


Transition Back to Optimized Solution 

The operator can move the arm out of the singularity region if the commanded velocity has 
a component in the Vz v direction (i.e., \&2 + p\ moves away from 90°), or in the Vy v direction 
(i.e., 1 64 1 moves away from 90°). Once outside the singularity region, both special solution 4 and 
the optimized solution for the entire commanded velocity are calculated. If the sign of 82 from 
the optimized solution matches the sign of 82 from special solution 4 and the sign of 84 from the 
optimized solution matches the sign of 84 from special solution 4, control switches back to the 
optimized method and calculation of the special solution is stopped. If the signs of the rates do 
not match, control using special solution 4 is continued. This transition back to the optimization 
method is used to prevent possible oscillations in the motion of the arm. 
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RESULTS AND DISCUSSION 

A real-time computer simulation was used to evaluate the optimized and special solution resolved 
rate equations developed in this paper (see appendix F). The current version of the simulation runs 
on a DEC VAX 11/750 computer and (optionally) interfaces to a GTI POLY 2000 graphics system 
which animates the motions of a 3-D model of the LTM. An operator used a six-axis joystick to 
issue velocity commands and watched the motions of the LTM model to assess the equations. For 
the motions simulated, the robot hand moved as commanded and the special solution equations 
appeared reasonable. 

Time histories of selected simulations of the LTM are presented here to add credence to the 
resolved rate equations and to examine use of the special solutions for the joint angle rates. The 
data in this paper were taken at a simulation time step of 1/16 sec. 

Using Dubey’s Method With Performance Criterion 

The joint angle rate solution calculated with Dubey’s method is a trade-off between trying to 
minimize the sum of the squares of the joint angle rates and trying to configure the arm to optimize 
a performance criterion of joint angles. The trade-off depends on a scalar weighting factor k. As 
|fc| — ► 0, preference is given to minimizing the rates. 

The effect of different values of k is shown in figures 10, 11, and 12 for an example performance 
criterion (appendix C). In figure 10, k = 0; in figure 11, k = -1; and in figure 12, k = -2. 
(Negative values of k mean that the tendency is to minimize the performance criterion.) Minimizing 
the performance criterion tends to keep 9 2 , #4, and 9 6 close to 0°, or away from 90° (away from 
singularities). The initial configuration of the arm and commanded velocity of the hand for figures 10 
to 12 are listed in table 111(a). The commanded velocity is exactly produced by the movement of the 
arm in each figure. However, as can be seen by comparing corresponding figures, the time histories 
of the joint angles and joint angle rates are quite different. 

In figure 10, the performance criterion is ignored (fc = 0) in the computation of a least-squares 
solution for the joint angle rates. In figures 10(a) and (b), the joint angles 62 , 9±, and 9$ move away 
from 0°. The index m is 1 throughout the simulation run. The results in figure 10 were verified by 
a generalized matrix inverse (via singular- value decomposition). 

Notice in figures 11 and 12, as k takes on the values -1 and -2, the tendency of 0 2 , #4 1 and 0$ to 
move away from 0° becomes more pronounced in comparison with figure 10. The index m switches 
from 1 to 2 and back to 1 in these figures as 9 4 moves through 0° (logic in fig. 4). 

As more precedence is given to minimizing the performance criterion of joint angles, the norm of 
6 increases. This is indicated in figure 13, which shows the effect of k on the time history of ||0|| for 
k = 0 (fig. 10), k — — 1 (fig. 11), and k = -2 (fig. 12). It should be noted that, in general, the curve 
for k = 0 may not remain below the curves for other values of fc, because as the joint angles change, 
different trajectories are being compared. However, if k = 0 is used anywhere on a curve generated 
for a nonzero value of fc, a lower value of ||0|| will be computed in this vicinity. 

Inherent Error in Special Solutions 

Four special solutions for arm joint angle rates have been developed as alternatives to using a 
generalized inverse solution for joint angle rates when the configuration of the arm is such that 
Dubey’s method does not apply. The reason for alternate solutions is that the generalized inverse 
solution can cause undesirable oscillations of the arm in these configurations and is computationally 
intensive. Oscillations are avoided by the special solutions, at the expense of allowing an error 
between the commanded velocity of the hand and that which is actually produced. It is assumed 
that a human operator can compensate for this error as time progresses; however, if continuous 
positional accuracy is an important issue , the special solutions are not applicable . Some examples of 
the error produced by the special solutions are presented. 

Errors Due to Special Solution 1 

Joint angles, joint angle rates, and the actual velocity of the hand for full extension of the ¥m 
using a generalized- inverse solution and using special solution 1 are shown in figures 14 and 15, 
respectively. The initial configuration of the arm and the commanded velocity of the hand for 
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figures 14 and 15 are listed in table 111(b). Oscillations which occur when using a generalized inverse 
solution at full extension are evident in figure 14. These oscillations are due to the integration time 
step (1/16 sec) used in numerically integrating the joint angle rates. A reduction in the oscillations 
will result by decreasing the integration time step. However, use of the generalized inverse very close 
to arm singularities can result in sluggish arm response and compromised hand velocities. 

Special solution 1 eliminates oscillations, and, as shown in figure 15(e), there is very little 
extraneous motion of the hand. Of the four special solutions developed in this paper, special 
solution 1 produces the most acceptable response of the arm. The reader should also note that the 
motion of the elbow upon retraction of the arm (which may be important in avoiding obstacles, for 
example) can be anticipated by the operator if special solution 1 is used. If further experimentation 
reveals that it is better to use the elbow pitch joint 193 (or a combination of the elbow joints #3 and 
64 ) instead of the elbow yaw joint $4 to retract the arm, special solution 1 can be modified to perform 
the desired elbow motion. When the generalized inverse is used to retract the arm, the motion of 
the elbow cannot be anticipated by the operator in most cases. 

Errors Due to Special Solutions 2, 3, and 4 

Commanding an outward thrust of the hand when the arm is fully extended makes no sense, 
and thus the consequences of ignoring commanded outward thrust when the arm is fully extended 
(special solution 1) are slight. On the other hand, ignoring a component of the commanded velocity 
in other singular regions (special solutions 2, 3, and 4) can produce unfavorable errors in the motion 
of the arm. Fortunately, using Dubey’s method tends to keep the arm out of these singular regions 
if the arm starts movement far enough away from the regions. This action happens because a least- 
squares solution for the joint angle rates attempts to keep the arm away from configurations that 
induce large joint angle rates. 

Special solutions 2, 3, and 4 provide the operator with a means of moving the LTM in and out 
of singularity regions in a controlled manner and without oscillations. The solutions are computed 
quickly and are applicable for real-time control. However, since they can result in erroneous motion 
of the arm, the angles 62 , 63, and 64— which define the spans of the singularity regions— should be 
made as small as practical to prevent the control program from switching to the special solutions 
most of the time. Some examples which characterize the nature of the errors are presented in 
figures 16 and 17. The initial configuration of the arm and the commanded velocity of the hand for 
figures 16 and 17 are listed in table III(c). 

Figure 16 shows time histories of joint angles, joint angle rates, and actual velocity of the hand for 
motion of the LTM near the singularity associated with special solution 2 (|^2 1 — 90° and #4 = 0°), 
but calculated with a generalized inverse solution. Oscillations occur as the arm passes close to the 
singularity, although components of the actual velocity are still in the correct proportion (the actual 
velocity of the hand is a scaled version of the commanded velocity due to joint angle rate scaling). 
Figure 17 corresponds to figure 16, except that special solution 2 is used. The arm does not oscillate, 
but there is significant error in the actual velocity of the hand when the control program switches 
to special solution 2, as shown in figure 17(e). (Recall that the component Vx$ of the commanded 
velocity is ignored for special solution 2. This component is approximately equal to the commanded 
velocity Vz (75 mm/sec) for this simulation.) Once the arm enters the singularity region associated 
with special solution 2, it remains in the region because the component of the commanded velocity 
which can move it out of the region — V\ in this case — is zero. An operator can compensate for such 
errors by simply moving the arm far enough out of the singularity region that control switches back 
to Dubey’s method, and then commanding a different approach path to the point of interest. 

Figures 18, 19, and 20 are included here to show how subtle changes in the configuration of the 
arm in singularity regions can vary the response of the hand. In all three figures, the arm is in 
the singularity associated with special solution 4 (|02 + = 90° and (#4! = 90°). Only the initial 

position of the elbow pitch joint (#3) is different in each figure. (For special solution 4, #3 does not 
translate the hand (at the wrist) so that, regardless of the value of #3, the hand remains at the same 
point in the workspace.) The commanded motion of the hand is also identical for the three figures 
and is expressed in base coordinates. The initial configuration of the arm and commanded velocity 
of the hand for figures 18, 19, and 20 are listed in table 111(d). 
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In figure 18, the commanded velocity does not have a component in the direction ignored by 
special solution 4. Therefore, there is very little error in the actual velocity of the hand, as shown 
in figure 18(e). 

Figure 19 is a time history of the response of the arm with conditions identical to those of figure 18, 
except that the elbow pitch joint is at 45° initially. For this simulation, a portion of the commanded 
velocity is in the direction which is ignored by special solution 4; thus, there is significant error in 
the actual velocity of the hand (fig. 19(e)) until control switches back to Dubey’s method at 0.45 sec. 

Figure 20 is a time history of the response of the arm with conditions identical to those of 
figures 18 and 19, except that the elbow pitch joint is at 85° initially. In this case, practically all the 
commanded velocity is in the direction which is ignored by special solution 4; thus, the hand moves 
very little. Therefore, the operator must command the arm to move in another direction to get it 
out of the singularity region before the desired motion can be accomplished. 

It is emphasized that the special solutions developed in this paper are by no means the only 
possible approaches for coping with singular configurations of the arm in which Dubey’s method 
does not apply. They are presented here to give a physical interpretation to the problems which 
occur when the arm reaches the singular configurations and are intended to represent a starting 
point for controlling the LTM with the assurance that real-time performance can be maintained and 
that damage to the hardware due to high-frequency oscillations of the arm will not occur. 

CONCLUDING REMARKS 

A set of optimized resolved rate equations have been developed for real-time control of the 
seven-degree-of- freedom Laboratory Telerobot ic Manipulator (LTM). The equations, which are based 
on a recent innovative optimization scheme developed at the Oak Ridge National Laboratory, 
represent a trade-off between two solutions: (1) a least-squares solution for the joint angle rates 
to produce a commanded velocity of the hand and (2) a solution to minimize joint angle rates while 
compromisingly configuring the manipulator to satisfy a performance criterion of the joint angles. 

A problem with using the scheme to formulate control equations is that it requires selecting a 
column of the Jacobian matrix so that the remaining six columns (or equivalently, six joint angle 
rates) are independent. A method for determining quickly which column to select is presented in this 
paper. But, such a selection is not always possible in which case the scheme does not apply — and 
alternate control equations were devised. 

A three-dimensional graphics model of the LTM was driven in response to velocity commands 
from a six-axis hand controller to assess the equations developed in this paper. For the motions 
simulated, the robot hand moves as commanded and the singularity fixes appear reasonable. 


NASA Langley Research Center 
Hampton, VA 23665-5225 
August 15, 1989 
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APPENDIX A 


MATRICES AND VECTORS ASSOCIATED WITH THE LTM 
Homogeneous Transformation Matrices 

Homogeneous transformation matrices are commonplace in the description of robotic manipula- 
tors. In general, the homogeneous transformation matrix from coordinate system i to coordinate 
system 2 — 1 is (ref. 7) 

cos 9\ — cos a* sin 0^ sina^sin^ 

sin 0 • cos oti cos 9[ —sin cos 0' 

0 sin cti cos c*i 

0 0 0 

which is expressed in terms of Denavit-Hartenberg parameters a^, dj, a;, and 0' (in ref. 7, 9 X replaces 
9[). The three parameters d x , and a x are constants, and 9[ is the variable joint angle. The 
rotational part of the transformation matrix A- -1 is the upper-left 3x3 submatrix denoted as 

-cos o.{ sin 0j sinaisin0 t - ’ 

cos cos 0^ — sinajcos0' (A2) 

sin a x cos a{ 

The position vector associated with A\_ x is 

r cos 0j ' 

p- _1 = \ o>i sin 9\ ► (A3) 

t dj j 

which is expressed in axis system i — 1 and is directed from axis system i — 1 to axis system i . 

The robot’s motion is usually expressed in terms of joint angles 0* that are initially referenced to 
some initial position. But, the matrix A-^ in equation (Al) is expressed in terms of the Denavit- 
Hartenberg joint angle 0-. To switch joint angle descriptions, make the substitution 

= 0 ? + Pi (A4) 

where /?,* is a constant offset bias to account for different starting positions. 

Homogeneous Transformation Matrices for the LTM 

The home position and axis systems for the LTM are shown in figure 2. Denavit-Hartenberg 
parameters for the LTM are shown in table I. For this paper, 0' = 0j, except for the matrix A®, in 
which 0g = 06 + 90° (i.e., = 90°, all others are zero). Note that the parameter d-j, which locates 

the hand axis system from the wrist, is considered zero in this paper {Ijjw = 0). 

Using these parameters and replacing the angles 0' with the angles 0 except for 0g, which is 
replaced with 06 + 90°, the transformation matrices A^ become 


"cos 0- 
RU= sin#- 
. 0 





vC\ 0 —S\ 0-1 

Si 0 Cl 0 

0-100 

. 0 0 0 1 . 


(A5) 
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(A6) 


~C 2 0 S2 IesC 2" 
*2 _ S 2 0 -C2 IesS 2 

Al 010 0 

.00 0 1 . 

rC 3 0 - 5 3 0-1 

*3 _ ^3 0 C3 0 

2 0-100 

. 0 0 0 1 . 

'C4 0 S4 1 ]/veCa~ 

a 4 _ 54 0 -C4 

“ 0 1 0 0 

. 0 0 0 1 . 

■C5 0 — S5 0" 

a 5 _ 0 C5 0 

4 “ 0 -1 0 0 

. 0 0 0 1 . 

"—56 0 Cfy 0 " 

*6 Cq 0 56 0 

“ 0 10 0 

. 0 0 0 1 . 

C 7 - 5 7 0 0 - 

a 7 Sj C7 0 0 

~ 0 0 10 

.0 0 0 1 . 


(A 7 ) 

(AS) 

(AS) 

(A10) 

(All) 


where C* means cos 8{ and S t means sin 9 t . 

Calculation of Hand-to-Base Transformation 

Commanded translational velocities in the hand axis system are transformed down to the base 
axis system of the robot arm when controlling the LTM. Consequently, the transformation matrix 
from the hand axis system to the base axis system must be calculated each time the joint rates 
are updated. The hand-to-base transformation (the transformation from axis system seven to axis 
system zero in fig. 2) is the product of the seven homogeneous matrices. That is, 

Ao = AqAj A2A3A4A5A6 (A 12 ) 

This same transformation can be calculated in fewer operations as follows. 

The rotational part of the matrix AJ is 



where A';, Y t , and Z* are the axes of coordinate system i expressed in coordinate system j. For 
example, 

= Xi | ?i | Zi (A14) 

where the caret ( ' ) means that the vector is expressed in the base axis system. Therefore, from 
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equation (A5), the Z\ axis expressed in base coordinates is 


Similarly, 


Zl = 


-Si 

c 1 
0 


) 


t>2 p lp2 

± 1 0 — — 


*2 | *2 j Z 2 


(A 15) 


(A 16) 


Multiplying the rotational parts of the matrices Aq and Aj given by equations (A5) and (A6) and 
substituting the result into equation (A16) yields the following expressions for the X 2 and Z 2 axes 
expressed in base coordinates: 



The X 3 axis expressed in base coordinates is 


CiC 2 ) 

SiC 2 > (A17) 

-S 2 J 

CiS 2 ) 

SiS 2 > (A18) 

<?2 J 


X 3 = RoRjR 2 


1 

0 

0 



= c 3 x 2 + s 3 r 2 

However, V 2 projects totally to Z\\ 



c 2 

1 

0 1 

s 2 

R?Vz = 

S '2 

1 

0 

-C 2 


0 

1 1 
! 1 ! 

0 


0 

0 

1 


Z\ 


(A19) 


(A20) 


Therefore, expressed in any common axis system, Y 2 and Z\ are the same, 
equation (A19) can be written as 

*3 = C 3 x 2 + SsZi 


Hence, T 2 = Z\, and 


(A21) 


where X 2 is given by equation (A17) and Z\ is given by equation (A15). The following coordinate 
system axes can be calculated in a manner similar to the calculation of A3: 


Z 3 = -S3A2 + C 3 Z\ (A22) 

X 4 = C4A3 - S 4 Z 2 (A23) 
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Z_\ = S 4 X 3 + (4 Z 2 
X'y = C5X4 + S5Z3 

= -S 5 .X 4 + CbZ'A 
X§ = —S^Xb — C(jZ 4 
Z 6 = C 6 X 5 -S 6 Z 4 
X 7 = CjXq + S-jZb 
Y-j = -S 7 X 6 + CjZb 
Z 7 = Zq 

The rotational part of the hand-to-base transformation is formed as 


R-n = 


x 7 ! y 7 I z 7 


(A24) 

(A25) 

(A26) 

(A27) 

(A28) 

(A29) 

(A30) 

(A31) 


(A32) 


where X 7 , Y 7 , and Z 7 are calculated by equations (A15), (A17), (A18), and (A21) to (A31). 

As seen in figure 2, the X 2 axis is always aligned with the shoulder-to-elbow link of the LTM. 
Therefore, the position vector from the origin of the base axis system to the origin of the (X 2 y 2 ,Z 2 ) 
axis system expressed in base coordinates is 

P2 = Ie$X 2 ( A33 ) 

where l is length of the elbow- to-shoulder link listed in table I, and A ~2 is calculated from 
equation (A17). Also, the X 4 axis is always aligned with the wrist-to-elbow link of the LTM. 
Therefore, the position vector from the origin of the (X 2 , Y 2 ,Z 2 ) axis system to the origin of the 
(X 4 ,Y 4 ,Zi) axis system expressed in base coordinates is 

Pi = IweXa ( A34 ) 


where Iq® is length of the wrist-to-elbow link listed in table I, and X 4 is calculated from 
equation (A23). The position vector from the base axis system to the (X 4 ,Y 4 ,Z 4 ) axis system 
expressed in base coordinates is ^ 

pS - p$ + p5 < A35 > 

Since in this paper the hand-to-wrist distance (Ihw 2) is zero, 


P7 = P6 = P5 = P4 


(A36) 


So, from equations (A32) and (A36), the hand-to-base transformation is 




0 ! 1 


( A3 7) 


Equations (A32) and (A33) to (A36) may be used to compute the hand-to-base transformation 
matrix more efficiently than equation (A12). Several of the vectors calculated in this appendix are 
also needed when computing the Jacobian matrix for the LTM as discussed in appendix D. 
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APPENDIX B 


EXTENDED APPLICATION OF EQUATIONS 


The resolved rate equations in the main text take the robot hand velocity (at wrist) as an input 
and calculate joint angle rates to produce this commanded velocity. This appendix explains how to 
use the equations in a more general setting, namely in moving an object relative to some arbitrarily 
specified axis system. The equations in this appendix are based on the general control structure for 
one or more robot arms described in reference 8. 

Reference Frames 

A robot hand and an object (held by the hand) are considered as one composite body; that is, 
hand and object move as a single body. The object may be the hand itself. An operator specifies the 
location and orientation of an axis system on the composite body, called the moving reference frame 
(mrf) in reference 8. The relationship between the hand axis system and the mrf remains fixed as 
the robot hand moves. 

An operator also specifies an axis system for his inputs, called the control reference frame (erf) 
in reference 8. Operator inputs represent the commanded velocity of the composite body (mrf), 
expressed in the erf. The commanded velocity is used to compute the velocity that the hand should 
have so that the object moves as desired. 

The object axis system (mrf) plays no role other than to influence the commanded velocity. For 
example, in teleoperation, an operator watches the movement of an object as he makes velocity 
inputs to control its motion. Or, the presently known position of the object axis system may be 
known and a new position is desired, in which case the commanded velocity is based on a positional 
error. 

Computing Hand Velocity To Move Object 

Velocity is commanded in the erf to move the object; but, once issued, this velocity represents the 
velocity of the composite body (hand and object). For example, when the object rotates about a line 
in the control reference frame, the composite body rotates about the line with the same rotational 
velocity as the object. Thus, the hand must also rotate with this commanded velocity. Hence, the 
velocity that is used to command movement of the object can be used directly to tell the hand how 
to move. Toward this end, form the matrix 

f R crjr I n har 

^hand I P crf 

.0 0 0 | 1 

where the matrix A{^ e d is a known function of the arm configuration (joint angles), and the matrix 

A^ e ls specified by the operator. 

Since the hand and object are a composite body, 


a cr f 
A hand 




hand] 
L base J 


1 A«/ 

A base 


(Bl) 


^hand — ^hand^cr/ (B2) 

where u > cr j is the commanded rotational velocity of the object. Then, the translational velocity of 
the hand is 

Vhand = KL V crf ~ ^hand X p^ nd (B3) 

where V cr y is the commanded translational velocity of the object, and p^ nd is the moment arm from 
the hand to the crf. The translational and rotational hand velocities, Vh an d an d ^hand> respectively, 
computed in equations (B2) and (B3) are used in resolved rate equations to cause the commanded 
movement of the object with respect to the control reference frame. 
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APPENDIX C 


EXAMPLE PERFORMANCE CRITERION (H(0)) 


Suppose the intent is to avoid arm and wrist singularities of the LTM by keeping |#2|> |#4l> and 
|0 6 | away from 90° whenever possible. For this purpose, define the performance criterion H(0) as 

H(0) = i (sin 2 62 + sin 2 #4 + sin 2 9q j (Cl) 

The gradient of the performance criterion is 

an/de 1 'i r o 

dll / 89 2 sin 62 cos O2 

dE/d9 3 0 

<911 / D0.\ _ sin 9 4 cos 64 


dH/dOr, 0 

<911 / 00(y sin cos 0% 

dE/dO-! J l 0 

To keep |0 2 |, (^4), and |0g| away from 90°, H(0) is minimized; i.e., the rate-of-convergence constant 
k should be negative. Assume k = — 1. In applying the performance criterion, equation (C2) is 
used in computing fcVH in equation (3), fcJiVH arm in equation (15), fcVH arm and fcJ3VH wr ist in 
equation (16), and fcVH a rm in equation (33). 

The reader should note that when joint angle rates are optimized (with eq. (3) or (33)), the arm 
may continue to move even when the commanded velocities of the hand are zero. Motion will occur 
if a configuration of the arm can be reached which minimizes H(0) without moving the hand. This 
action may not be desirable. To ensure that the arm does not move when the commanded velocity 
of the hand is zero, the rate-of-convergence constant is assigned as follows: 

f 0 (| V| < 6y and |u;| < 6 w ) 

k = < 

( k (Otherwise) 

where 6y and are small positive deadbands. The magnitude of k may also be changed to speed or 
slow the rate of convergence. However, large values of k can cause oscillations as the robot arm nears 
the joint configuration which minimizes the performance criterion (ref. 9). The maximum value of 
k which can be used depends on the integration time step size. 
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APPENDIX D 


COMPUTING Ji, J 3 , AND SOLUTIONS OF WRIST EQUATIONS 

Joint angle rates of a robot arm cause the hand to move with velocity i, described by the familiar 
kinematic equation 

x = J0 (Dl) 

If a robot has a three-axis, spherical wrist, and the hand axis system is located at the wrist, 
equation (Dl) can be partitioned as 



where V is the commanded translational velocity of the hand, u> is the commanded rotational velocity 
of the hand, 0 arm is a vector of the arm joint angle rates (which translate the hand), and 0 wr ist is a 
vector of the wrist joint angle rates. 

For the LTM, J\ and J2 are 3 by 4 matrices, and J3 is a 3 by 3 matrix. The joint angle rate 
vector 0 arm is a vector of the joint angle rates 0 \, 02, ^3, and 84 ; and the joint angle rate vector 
0 W rist a vector of the joint angle rates #5, and 9j. A method for real-time computation of 
an optimized 0 is described in the main text. For singular configurations of the arm or wrist in 
which the optimization method does not apply, special solutions for 0 arm and 0 wr i st are presented. 
Explicit computation of the matrices Ji and J3 (which is necessary when using the optimization 
method) is discussed in this appendix. A general method for solving wrist equations (14), (19), and 
(34) (when optimizing joint angle rates) and wrist equation (36) (when special solutions are used) 
is also presented. 

Computation of Ji in Base Axis System 

The translational velocity of the robot hand is the vector sum of the translational-velocity 
contributions from each of the arm joint angle rates. Consequently, the Jacobian submatrix Ji 
in equation (D2) has the form: 


Ji = 


Zq X p? 



Z 2 x pf 


Z3 X Pj 


(D3) 


where the caret (*) signifies that the vector is expressed in the base axis system. The position vector 
P7 extends from the origin of axis system i to the origin of axis system 7 (the hand axis system). 
Physically, Zj, whose elements are the first three entries in the third column of the homogeneous 
transformation matrix Aq, is a unit vector directed along the rotational axis of joint i - hi. 

Referring to figure 1, the position vectors py and pj are equal to the position vector from the 
base axis system to the (X^K^Z,*) axis system: 

P? = P 7 = P 4 (D 4 ) 

and the position vectors pj and p2 are equal to the position vector from the (X 2 X 2 X 2 ) axis system 
to the (X 4 X 4 X 4 ) axis system: 

P7 = P7 = P4 (D5) 

Therefore, the Jacobian submatrix J j may be expressed as 


Jl = 


Zq x p^ 



(D6) 
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The position vectors p<j and pj are assumed already known from equations (A35) and (A34), 
respectively, in appendix A. Furthermore, the vector Zq is the first three elements of the third column 
of the identity matrix Aq (base axis system transformed to itself), and the remaining vectors Zj, 
Z 2 , and Z3 in equation (D6) are assumed already known from equations (A15), (A18), and (A22), 
respectively. 


The first column of J \ is 



= Z 0 X = 



(D7) 


The second column of Ji is 


r JiMn 

f _Sl l 

r Cip\\z\ 

< JiM 

> = Zi x p° 4 = l Ci > x p^ = < 

SipIm 

,Jl[3,2]J 

l 0 J 

1 -C 1 P 4 W - ■S’lp^y] > 


(D8) 


The third column of Ji is 


The fourth column of J 1 is 


' J 1 [1 , 3] 1 


' Z 2 [y ] P4 [z] - Z 2 [z] p\\y) 1 

< Jl[2,3] 

> = Z 2 x p] = | 

Z 2 \z\ p|[x] - Z 2 [x) p\[z] > 

k Jl[3, 3] J 


, Z 2 [x] p \[y\ - Z 2 [y] p|[x] J 

Jl is 

' Ji[D 4] "I 


' Z 3 [y] p \{z} - Z 3 [z] P4 [y] ' 

< Jl[2,4] 

> = Z 3 x pi = < 

%[z\ P4W - Z 3 [x] p l[z] > 

, Jl[3, 4] J 


. Z;i[x] p|[j 1 } - Z 3 [y] pftar] , 


(D9) 


(DIO) 


Equations (D7) to (DIO) are used to calculate the Jacobian submatrix Jj when solving equations (13) 
and (18) in the main text. 

Computation of J3 in Hand Axis System 

The rotational velocity of the hand contributed by the wrist is the vector sum of the three wrist 
joint velocities. Therefore, in equation (D2), the columns of J3 are the axes of rotation of the wrist 
joints: 


J:< = 


z 4 l z 5 \ z 6 


(Dll) 


When expressed in the hand axis system, the rotational axes of the wrist are 


V r)6p 5 d 4 

Z4 — 


Z 5 = R?Rj? { 0 


Z 6 =R 6 7 



(D12) 


(D 13) 


(D 14) 
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(See appendix A for the required rotational transformation matrices.) Expanding equations (D 12 ) 
to (D14) and substituting the results into equation (Dll) yield 


J 3 


’ -C-jCq S 7 O' 
S 7 C & C 7 0 

. -S 6 0 l. 


Equation (D15) is used to compute fcJ 3 VH wr j st in equation (16) in the main text. 

Solving Wrist Equations (14), (19), (34), and (36) 

Wrist equations (14), (19), (34), and (36) are all of the form 


(D15) 


7 J 2 ©arm — J3®wrist (D16) 

where 7 and ©arm are known and a solution is sought for ©wrist- In equation (14), 7 corresponds 
to u) *J 3 VH wrist , ©arm corresponds to d> p , ar m + fcVH arm , and 0 wr j st corresponds to d> p , wrist- 
In equation (19), 7 is 0, ©arm corresponds to <j>h,axmi an< I ©wrist corresponds to 4>h^i\st- 1° 

equation (34), 7 corresponds to w, 0 ar m corresponds to 0 O pt,armi and 0 wr ist corresponds to ©wrist - 
In equation (36), 7 corresponds to ©arm corresponds to 0 Atm , and ©wrist corresponds to ©wrist - 
In equations (14) and (19), J3 is invertible, and in equations (34) and (36), J3 is not invertible. A 
general method for computing a solution of equation (D16) (and, therefore, solutions of eqs. (14), 
(19), (34), and (36)), regardless of the invertibility of J3, is presented. 

J 2 ® arm in Hand Axis System 

In equation (D2), the contribution of the arm joint angle rates to the rotational velocity of the 
hand is J 2 ^arm- This contribution is the vector sum of the rotational velocities of the arm joints. 
Thus, 


J2^arm — 


R?Z 0 R \Zi 


R 2 7 Z 2 


R 3 7 z 3 


0a 


f 0 1 

R7 | 0 > + Rj < 

Ui J 

r»i 

ti) 

' +r? {!HI 

»} 

«i j 

d5d5 J r> 2 

K 7 K 3 < k 3 

R -2 

U- 

\ 

( ° ) (o)\ r 0 y 

° + ° +{ ° 
l ©1 J L ©2 J / l ©3 J 



(D17) 


Equation (D17) simply transforms the rotational rates of the arm joints to the hand axis system. 
The term J 2 ©arm in equation (D16) (analogous to J 2 0 a rm) is therefore expressed: 


J 2 © 


arm 


n5n5 

— K 7 K 3 



Equation (D18) is expanded by using two temporary vectors, 73 and 75. First, let 


(D18) 


73 


r -C3S 2 0 1+ S3©2 ) 

< — C 2 ©i — ©3 1 

, S3S2Q1 + C 3©2 + @4 J 


(D19) 
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diich takes rotational rates ©i, 0 2 , ©3, and 0 4 to axis system 3. Next, let 


r C5C473M + C 5 547 3 [y] + 5573(2] ' 
75 = < -S 473 M + ^473 fo] > 

, -55(7473(0:] - 5 5 5 4 73[y] + <7573(2] , 

which takes 73 to axis system 5. Finally, compute J2©arm as 

( -C 7 5 6 75 (x] + <776575(2/] + 5775(2] 


J'2@arm — \ 5 7 5575 [®] 5 7 <7 q 75 [l/] "h (7775(2] 
l 6 , 675M + 5675 M 

which takes 75 to the hand axis system. 

Components of 6> W rist 

Once J 2 @arm is calculated, the 6> wr j st is computed by inverting J 3 : 

©wrist. = Jo ^ 


(D20) 


(D21) 


(D22) 


where 

r = 7- j 2 © arm 

The expression for the submatrix J3 shown in equation (D15) is easily inverted as 

r c 7 — 5 7 0 ’ 

J 3 1 = -SiC 6 -C 7 Ce 0 
. C 7 S$ —S 7 S$ — (76. 

Therefore, equation (D22) can be written as the three simple scalar equations: 

a -c 7 r x + 57 rj, 

® 5 = cl 


© 6 = s 7 r x + c 7 r y 

©7 = r* + 56©5 


(D23) 


(D24) 


(D25) 


(D26) 

(D27) 


Notice that equation (D25) has a singularity at |0 6 | = 90°, which implies the same for matrix J 3 . 
To prevent a division by zero in equation (D25), the assignment 


is made whenever 


C 6 = Kq sign(<7 6 ) 


|C 6 | < K 6 


(D28) 


(D29) 


where Kq is an arbitrary small number (assumed 10 5 in this paper (ref. 10)). Rate scaling must 
also be employed to avoid large commanded wrist joint angle rates when using this approach. 
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APPENDIX E 


ANALYTIC EXPRESSION FOR Ji IN (A 2 ,Y 2 ^2) AXIS SYSTEM 


From appendix D, the columns of the Jacobian submatrix Jj for the LTM are 


Ji = 


Zo * P4 j Z\ * P 4 ! Z2 X P 4 j Z 3 X pi 


(El) 


As a convenience in the development of special solutions for singular arm configurations, J] is 
expressed in the (A^,!^!^) axis system — which means the vectors Zq, Z\, Z2, Z3, p®, and p| in 
equation (El) are expressed in the (A^,!^!^) axis system. These vectors are (see appendix A for 
the necessary rotational transformation matrices) 





l ES 

P2 = IesX 2 = ^ 0 
0 


P 4 = l WE X 4 = ^HeRJR-;} 


f C 4 C 3 lmS ) 

= \ C^Iwe / 

(. -S^WE J 


P4 — P2 + P4 = 


Ies + C^C^Iwe 1 

C4S3IWE > 

-S^ly/E ) 


(E2) 

(E 3 ) 

(E 4 ) 

(E 5 ) 

(E6) 

(E 7 ) 

(E8) 


Taking the appropriate cross products in equation (El) gives the expression for Ji in the (A^ YYZ^) 
axis system: 


Jl 


-C4S3C2IWE -S^lwE 

C2 Ies + (C4C3C2 — S 4 S 2 )Iwe 0 

-C 4 S 3 S21we -Ies ~ C 4C3IWE 


-C4S3IWE 

C4C3IWE 

0 


-S4C2IWE' 
-S4S3IWE 
-C4IWE - 


(E 9 ) 
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APPENDIX F 

COMPUTER PROGRAM FOR RESOLVED RATE CONTROL OF THE LTM 

A computer program which controls the LTM by using the optimization and special-solution 
resolved rate equations developed in the main text is described in a stepwise manner in this section. 
The commanded translational and rotational velocities of the hand are read as inputs (from a 
joystick, for example), and the joint angle rates necessary to produce the commanded velocities 
are calculated. These joint angle rates are then integrated and sent as joint angle commands to 
servo controllers of the LTM. The process of reading operator inputs, calculating joint angle 
rates, integrating the rates, and sending the updated (desired) joint angles to the servo controllers 
is repeated in a continuous loop. The time between successive reads of the operator inputs (i.e., the 
time to complete one iteration of the loop) is denoted At. 

Since joint angles are integrated over the time interval A £, the angles <5 2 i $ 3, and <5g , which 
determine the span of the singularity regions of special solutions, cannot have arbitrarily small values. 
Minimum values for these deltas are 

^min[^] = ®maxW A£ (FI) 

where 0 max [z] is the physical maximum (absolute value) joint angle rate which can be produced by 
joint z, and <5 m j n [z] is the minimum value for 6j. In this paper, 0 m ax[^] = 30 deg/sec for i = 1 to 7, 
and A t = 1/16 sec. The angles <5 2 , 63, 64, and are assumed to be 2.0°— slightly larger than their 
minimum values (30/16 or 1.875°). 

Given that the LTM is in a known configuration 0(t) at time t, the steps necessary to calculate 
the next set of desired joint angles 0(t -I- At) at time t + At are 

Step 1: Calculate the hand-to-base transformation A J and the Jacobian submatrix J 1 , expressed 
in base coordinates, as outlined in appendixes A and D. 

Step 2i Calculate V and u?, which are the translational and rotational velocities, respectively, 
of the hand axis system, expressed in the hand axis system, as discussed in appendix B, 
unless these are already known as operator commands in the hand axis system. Transform 
V to base coordinates with Aq that was calculated in step 1. 

Step 3: Use the logic outlined in figure 4 to determine which column of Ji (column m), if any, can 
be eliminated to form an invertible Ji (the equality conditions in fig. 4 should be replaced 
with the singularity regions defined in equations (37) to (40)). If an invertible J\ exists, 
proceed to step 4. If a singularity region has been entered, set a special solution flag to 
indicate that a special solution is being used. Then, calculate 0 a rm using the appropriate 
special solution, calculate 0 W rist f rom equation (36) as discussed in appendix D, and go 
to step 8. 

Step 4: If a performance criterion of joint angles is to be satisfied, calculate the gradient of the 
performance criterion times the weighting factor fcVH (see appendix C for an example). 
Also calculate fcJiVH ar m and fcJ3VH wrist with the Ji calculated in step 1 and the 
expression for J3 (in hand coordinates) in equation (30). 

Step 5: Assemble the particular and homogeneous solutions for the arm joint angle rates from 
equations (15) and (21) with arm[ m ] = 0 an d = 1* Cramer s rule or any 

other convenient method may be used to calculate 0 p , a rm an d 0/i, ar m- 

Step 6: If J3 is near its singularity (eq. (32) j, optimize the arm joint angle rates only and calculate 
the wrist joint angle rates required to produce the commanded rotational velocity of the 
hand (eqs. (33) and (34)). If J3 is not near its singularity, calculate the particular and 
homogeneous solutions for the wrist joint angle rates from equations (14) and (19), as 
discussed in appendix D, and calculate the optimized solution for the joint angle rates 
using equation (3). 
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Step 7: Check the condition of the special solution flag. If the special solution flag is not set, 
proceed to step 8. If the special solution flag is set, the arm has left one of the singularity 
regions defined in equations (37), (38), (39), and (40). If special solution 2, 3, or 4 was 
in use, calculate the arm joint angle rates for this special solution and then compare the 
appropriate arm joint angle rates of the special solution with the corresponding arm joint 
angle rates of the optimized solution calculated in step 6. If the signs of the corresponding 
rates match, reset the special solution flag, discontinue calculation of the special solution, 
and switch to the optimized solution. (Recall that for special solution 2, the sign of 62 
of the special solution must match the sign of 62 of the optimized solution; for special 
solution 3, the sign of O4 of the special solution must match the sign of 64 of the optimized 
solution; and for special solution 4, the signs of 62 and 64 of the special solution must 
match the signs of #2 and 64 of the optimized solution, respectively.) If the signs of the 
corresponding joint angle rates do not match, calculate 0 W rist from equation (36) and 
continue using the special solution. If special solution 1 was in use, reset the special 
solution flag, discontinue calculation of the special solution, and immediately switch to 
the optimized solution. 

Step 8: Scale the calculated joint angle rate solution, if necessary, as follows. Using the vector of 
physical maximum (absolute value) joint angle rates which can be produced by the joints 
of the robot, 0 m ax[i]> calculate 

= l^il ~ $maxH] (F2) 

for i = 1 to 7. If all e*’s are less than or equal to zero, the joint angle rates do not need 
to be scaled. If one or more are greater than zero, find the largest positive element of 
e — denoted kj — and scale the joint angle rates by using the corresponding 9j and 0 raa x[j] 
(ref. 10) as follows: 

9 = 6 (F3) 

\ e j\ 

Equation (F3) sets Qj to its physical maximum rate (with the proper sign), and propor- 
tionally scales the other rates (recall that <9 m ax[j] is assumed 30 deg/sec for k = 1 to 7 in 
this paper). 

Step 9: Integrate the scaled joint angle rates, for example, by using Euler integration, 

9{t + At) = 6(t) At + 9(t) (F4) 

or Adams-Bashforth second-order predictor integration, 

9(t + A t) = Y (30(0 - 9{t - At)) + 9(t) (F5) 

Step 10: If all the joint angles 9{t -I- At) are inside their corresponding physical joint angle limits, 
command the servo controllers of the LTM to move to the calculated joint angle positions, 
9(t + At) } and repeat the loop starting at step 1 with the updated joint angles. If any 
of the updated angles are outside their limits, do not move from the current position and 
repeat the loop starting at step 1 with the previous joint angle command {9(t)). 

The steps outlined in this section are not computationally intensive and calculate the optimized 
solution (when possible) without formally computing the generalized inverse of the Jacobian matrix 
(which is time consuming). 
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Table I. Denavit-Hartenberg Parameters for the LTM 



*Ies = Elbow-to-shoulder distance = 23 in. (5842 mm). 
^hvE — Wrist-to-elbow distance = 20 in. (5080 mm). 
^HW — Hand-to-wrist distance = 9 in. (2286 mm). 

(In the equations in the text, Ifjw is assumed to be 0.) 


Table II. Singularities Associated With the Four Jfs Formed by Deleting Column m of Ji 
[1^3 1 = 180° and |0 4 | = 180° are not physically realizable] 


Eliminated column 
of J i , m 

Determinant of remaining 3 by 3 
submatrix of Jj, det(Ji) 

Singularities 

1 


0 

0 

00 

*-H 

O 

O 

O 

03 

f- 

O 

O 

O 

II 

2 

“ ^ES l we $3 C *2 

|0 2 | = 90° 

| 0 3 | = 0 ° or 180° 
|0 4 | = 90° 

3 

IesIweS<\Cz[I es C 2 + hvE(C,\C-iC 2 - S 4 S 2 )] 

|0 2 | = 90° and (^ 4 1 = 0 ° or 180° 
|0 3 | = 90° 

| 0 4 | = 0 ° or 180° 

-IesC 2 = %e(C 4 C 3 C 2 - S 4 S 2 ) 

4 

l E sHveC $S %[1 [rgC 2 + Iwe{CaC$C 2 - 5452 )] 

|0 2 | = 90° and |0 4 | = 0° or 180° 
|0 3 | = 0° or 180° 

|0 4 | = 90° 

-IesC 2 — IwEi^C^Ci - S 4 S 2 ) 
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Table III. Initial Ann Configuration and Commanded Velocities for Figures 10 to 12 and 14 to 20 


Initial configuration 

Commanded velocity 
(a) 

(a) Figures 10, 11, and 12 

0, = -45° 0 5 = -45° 

02 = -45° 0 C = -10° 

0 3 r= 45° 0 7 = 0° 

6a = 10° 

Vx = 30 mm/sec w x = 10 deg/ sec 

Vy = — 30 mm/sec u)y = 15 deg/sec 

V/ — 0 mm/sec wz = — 10 deg/sec 

(b) Figures 14 and 15 

0, = 10° 0 5 = 10° 

0 2 = 10° 0 6 = 10° 

0 3 = -20° 07 = 0° 

04 = -20° 

Vx — 0 mm/sec ujx ~ 0 deg/sec 

Vy = 0 mm/sec u)y = 0 deg/sec 

V z — 75 mm/sec u) Z = 0 deg/sec 

(c) Figures 16 and 17 

0, = -45° 0 5 = 0° 

02 = 85° 0 6 = 0° 

0 3 = -45° 0 7 - 0° 

04 = 11.5° 

Vx — 0 mm/sec u>x = 0 deg/sec 

Vy — —50 mm/sec c oy = 0 deg/sec 

Vx = 75 mm/sec w z — 0 deg/sec 

(d) Figures 18, 19, and 20 

0, =0° 05 = 0° 

0 2 = 48.99° 0 6 = 0° 

*-03 = 0° 07 — 0° 

04 = 90° 

Vx — 0 mm/sec &x — 0 deg/sec 

Vy = 0 mm/sec (oy — 0 deg/ sec 

V z — —75 mm/sec — 0 deg/sec 


“Velocities in hand coordinates for parts (a), (b), and (c) and in base coordinates for part (d). 
6 0 3 = 0° in figure 18; 63 = 45° in figure 19; O 3 = 85° in figure 20. 
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Figure 1. Laboratory Telerobotic Manipulator (LTM). 
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Figure 4. Choosing component m for setting </>p,arm[^] = 0 and 0h,arm[ m ] — 1 in Dubey’s method or identifying 
special solutions. 
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Figure 5. Geometry for special solution 1: full extension via elbow pitch and elbow yaw. 



(b) Elbow is not pitched; $3 
Figure 5. Concluded. 
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Figure 7. Geometry for special solution 2: full extension via elbow yaw, at ±90° shoulder yaw. 




ii&m 


ORIGINAL' PAGET 

fc- BLACK AMD WHiT-E PHOIGGRARM 



49 









Time, sec 


(d) Wrist joint angle rates. 
Figure 11. Concluded. 
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(e) Actual velocity of hand (in hand coordinates). 
Figure 14. Concluded. 
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(a) Arm joint angles. 



Time, sec 


(b) Wrist joint angles. 

Figure 17. Motion of arm near the singularity 1 02 1 = 90° and 64 = 0 using Dubey s method and special 
solution 2. 











0 .25 .50 .75 1.00 1.25 1.50 

Time, sec 

(b) Wrist joint angles. 

Figure 18. Motion of arm near the singularity |02 + /x| = 90° and 64 = 0° with elbow pitch joint #3 = 0° using 
special solution 4. 









70 


Special 

solution 

4 


Dubey’s method, m - 1 



(a) Arm joint angles. 



(b) Wrist joint angles. 

Figure 19. Motion of arm near the singularity \$2 + /x| = 90° and 64 = 0° with elbow pitch joint 9 3 = 45° using 
special solution 4, 
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(e) Actual velocity of hand (in base coordinates) 
Figure 19. Concluded. 
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(a) Arm joint angles. 



Time, sec 


(b) Wrist joint angles. 

Figure 20. Motion of arm near the singularity |#2 + /i| = 90° and 64 = 0° with elbow pitch joint #3 = 85° using 
special solution 4. 
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